/*
 * @Description: 订阅激光点云信息，并解析数据
 * @Author: Zhijian Qiao
 * @Date: 2020-02-05 02:27:30
 */

#ifndef avp_slam_SUBSCRIBER_FourImagesSubscriber_HPP_
#define avp_slam_SUBSCRIBER_FourImagesSubscriber_HPP_

#include <deque>
#include <mutex>
#include <thread>

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include "sensor_data/four_images_data.hpp"
#include <sensor_msgs/CameraInfo.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/subscriber.h>

namespace avp_slam {
    typedef message_filters::Subscriber<sensor_msgs::Image> filter_image;
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::Image> ApproSynch;

    class FourImagesSubscriber {
    public:
        FourImagesSubscriber(ros::NodeHandle &nh, size_t buff_size);

        FourImagesSubscriber() = default;

        void ParseData(std::deque<FourImagesData> &images_data_buff);

    private:
        void msg_callback(const sensor_msgs::ImageConstPtr &depthFront, const sensor_msgs::ImageConstPtr &colorFront,
                          const sensor_msgs::ImageConstPtr &depthLeft, const sensor_msgs::ImageConstPtr &colorLeft,
                          const sensor_msgs::ImageConstPtr &depthBack, const sensor_msgs::ImageConstPtr &colorBack,
                          const sensor_msgs::ImageConstPtr &depthRight, const sensor_msgs::ImageConstPtr &colorRight);

    private:
        ros::NodeHandle nh_;
        std::shared_ptr<filter_image> cmr0_;
        std::shared_ptr<filter_image> cmr1_;
        std::shared_ptr<filter_image> cmr2_;
        std::shared_ptr<filter_image> cmr3_;
        std::shared_ptr<filter_image> cmr4_;
        std::shared_ptr<filter_image> cmr5_;
        std::shared_ptr<filter_image> cmr6_;
        std::shared_ptr<filter_image> cmr7_;
        std::shared_ptr<message_filters::Synchronizer<ApproSynch>> appro_synch_;
        std::deque<FourImagesData> new_images_data_;
        std::mutex buff_mutex_;
    };
}

#endif